/*=========================================================================
  UberSim Source Code Release
  -------------------------------------------------------------------------
  Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
  James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
  Erick Tryzelaar {erickt}@andrew.cmu.edu
  School of Computer Science, Carnegie Mellon University
  -------------------------------------------------------------------------
  This software is distributed under the GNU General Public License,
  version 2.  If you do not have a copy of this licence, visit
  www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
  Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
  in the hope that it will be useful, but WITHOUT ANY WARRANTY,
  including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  -------------------------------------------------------------------------*/

#include <string>

#include <drawstuff/drawstuff.h>

#include <time.h>
#include <sys/time.h>
#include <unistd.h>

#include "../reality/net_vision.h"
#include "../utils/configreader.h"
#include "../utils/socket.h"
#include "../utils/vtracker.h"

#include "mmgr.h"

#include "Ball.h"
#include "CollisionGroup.h"
#include "CollisionManager.h"
#include "DiffRobot.h"
#include "Engine.h"
#include "Entity.h"
#include "Event.h"
#include "Field.h"
#include "Math.h"
#include "OmniRobot.h"
#include "RigidBody.h"
#include "Timer.h"


/*******************************/

CR_DECLARE (TARGET_FPS);
CR_DECLARE (SKIP_FRAME);
CR_DECLARE (USER_ROBOT_VELOCITY_STEP);
CR_DECLARE (USER_ROBOT_ANGULAR_VELOCITY_STEP);
CR_DECLARE (USER_ROBOT_POSITION);
CR_DECLARE (BALL_POSITION);
CR_DECLARE (POSITION_SCALE);
CR_DECLARE (VELOCITY_SCALE);
CR_DECLARE (ANGULAR_VELOCITY_SCALE);
CR_DECLARE (X_LOWER_LIMIT);
CR_DECLARE (X_UPPER_LIMIT);
CR_DECLARE (Y_LOWER_LIMIT);
CR_DECLARE (Y_UPPER_LIMIT);
CR_DECLARE (ROBOT_START_HEIGHT);
CR_DECLARE (RADIO_LATENCY);

/*******************************/

struct Observation 
{
  double timestamp;
  vraw   ball;
  vraw   robots[NUM_TEAMS][MAX_TEAM_ROBOTS];
  bool ball_kicked;
};

/*******************************/

VisionFrameMsg visionFrame;
VTracker    tracker;
Socket      visionServer (NET_VISION_PROTOCOL, NET_VISION_ACK_PERIOD);
Observation obs;

Engine*         engine         = 0;
CollisionGroup* collisionGroup = 0;

Entity* robot = 0;
Ball*   ball  = 0;
Entity* robotTeams[NUM_TEAMS][MAX_TEAM_ROBOTS];
bool    robotMsgRecieved[NUM_TEAMS][MAX_TEAM_ROBOTS];

Vector3 targetLinearVelocity  = Vector3::ZERO;
Vector3 targetAngularVelocity = Vector3::ZERO;
bool    isDribblerEnabled = false;
bool    isKickerEnabled   = false;

bool isShowingFps = false;

/*******************************/

void makeObservations      (Observation& obs);
void updateTracking        (Observation& obs);
void visionRecieveMessages ();
bool visionSendMessages    (Observation& obs);

/*******************************/

void step(int pause)
{
  static int call_count = 0;

  engine->beginFrame ();
  engine->frame      ();
  engine->endFrame   ();

  if (!engine->getIsSkippingFrame () && 
      (!engine->getIsPaused () || engine->getIsTakingOneStep ()))	{	

    // HACK: to prevent ball from popping out
    if (ball)	{
      Vector3 v = ball->getMotionController ()->getWorldVelocity ();

      if (sqrt(v.x * v.x + v.y * v.y) > 100.0)	{
        printf ("ball popped up, fixing: %f %f %f\n", v.x, v.y, v.z);
        v.y = 0;
        v.x = 0;
        ball->getMotionController ()->setWorldVelocity (v);
        //exit (0);
      }
    }

    // HACK: to prevent robots from popping out
    if (ball) {
      if (robot) {
        Vector3 v  = robot->getMotionController ()->getWorldVelocity ();
        Vector3 av = robot->getMotionController ()->getWorldAngularVelocity ();

        if (Math::abs (av.x) > 5.0 || Math::abs (av.y) > 5.0 || v.z > 5.0){
          printf ("driving robot exploded: v: %f %f %f av: %f %f %f\n", 
                  v.x,  v.y,  v.z,
                  av.x, av.y, av.z);
					
          v  /= 100;
          av /= 100;
					
          robot->getMotionController ()->setWorldVelocity        (v);//Vector3::ZERO);
          robot->getMotionController ()->setWorldAngularVelocity (av);//Vector3::ZERO);
          //exit (0);
        }
				
        Vector3 z = robot->getMotionController ()->getWorldRotation () * Vector3::UNIT_Z;
				
        //printf ("z: %f\n", z.dot (Vector3::UNIT_Z));
        if (z.dot (Vector3::UNIT_Z) < 0) {
          printf ("driving robot flipped, fixing\n");
				
          Vector3 rotation = robot->getMotionController()->getWorldRotation ().eulerAngles ();
				
          robot->getMotionController()->setWorldRotation(Matrix33::rotateAboutZ(rotation.z));
          //exit (0);
        }
      }
    }

    for (int i = 0; i < NUM_TEAMS; i++)	{
      for (int j = 0; j < MAX_TEAM_ROBOTS; j++)	{
        Entity* robot = robotTeams[i][j];

        if (robot) {
          Vector3 v = robot->getMotionController ()->getWorldVelocity ();
          Vector3 av = robot->getMotionController ()->getWorldAngularVelocity ();

          if (Math::abs (av.x) > 5.0 || Math::abs (av.y) > 5.0 || v.z > 5.0) {
            printf ("robot[%d][%d] popped: v: %f %f %f av: %f %f %f\n", i, j,
                    v.x,  v.y,  v.z,
                    av.x, av.y, av.z);

            v  /= 10;
            av /= 100;

            //Vector3 rotation = robot->getMotionController ()->getWorldRotation ().eulerAngles ();
				
            robot->getMotionController ()->setWorldVelocity        (v);//Vector3::ZERO);
            robot->getMotionController ()->setWorldAngularVelocity (av);//Vector3::ZERO);
            //robot->getMotionController ()->setWorldRotation        (Matrix33::rotateAboutZ (rotation.z));
            //exit (0);
          }
					
          Vector3 z = robot->getMotionController ()->getWorldRotation () * Vector3::UNIT_Z;

          if (z.dot (Vector3::UNIT_Z) < 0) {
            printf ("driving robot flipped, fixing\n");
					
            Vector3 rotation = robot->getMotionController ()->getWorldRotation ().eulerAngles ();
				
            robot->getMotionController ()->setWorldRotation (Matrix33::rotateAboutZ (rotation.z));
            //exit (0);
          }
        }
      }
    }
  }


  if (!engine->getIsSkippingFrame ()) {

    call_count++;

    if (call_count % IVAR (SKIP_FRAME) == 0) {
      makeObservations(obs);
      updateTracking(obs);
	    
      if (!visionSendMessages (obs)) {
        fprintf (stderr, "Cant send vision\n");
        exit    (-1);
      }
	    
      visionRecieveMessages ();
    }
  }
	
}

/*******************************/

void command (int cmd)
{	
  if (robot) {
    switch (cmd) {
      case 'w':
      case 'W':
        {
          targetLinearVelocity += Vector3 (DVAR (USER_ROBOT_VELOCITY_STEP), 0.0, 0.0);
          robot->handleEvent (new VelocityEvent (targetLinearVelocity, targetAngularVelocity));
        }
        break;
      case 's':
      case 'S':
        {
          targetLinearVelocity -= Vector3 (DVAR (USER_ROBOT_VELOCITY_STEP), 0.0, 0.0);
          robot->handleEvent (new VelocityEvent (targetLinearVelocity, targetAngularVelocity));
        }
        break;
      case 'a':
      case 'A':
        {
          targetLinearVelocity += Vector3 (0.0, DVAR (USER_ROBOT_VELOCITY_STEP), 0.0);
          robot->handleEvent (new VelocityEvent (targetLinearVelocity, targetAngularVelocity));
        }
        break;
      case 'd':
      case 'D':
        {
          targetLinearVelocity -= Vector3 (0.0, DVAR (USER_ROBOT_VELOCITY_STEP), 0.0);
          robot->handleEvent (new VelocityEvent (targetLinearVelocity, targetAngularVelocity));
        }
        break;
      case 'q':
      case 'Q':
        {
          targetAngularVelocity += Vector3 (0.0, 0.0, DVAR (USER_ROBOT_ANGULAR_VELOCITY_STEP));
          robot->handleEvent (new VelocityEvent (targetLinearVelocity, targetAngularVelocity));
        }
        break;
      case 'e':
      case 'E':
        {
          targetAngularVelocity -= Vector3 (0.0, 0.0, DVAR (USER_ROBOT_ANGULAR_VELOCITY_STEP));
          robot->handleEvent (new VelocityEvent (targetLinearVelocity, targetAngularVelocity));
        }
        break;
      case 't':
      case 'T':
        {
          isDribblerEnabled = !isDribblerEnabled;
          robot->handleEvent (new DribblerEvent (isDribblerEnabled));
        }
        break;
      case 'z':
      case 'Z':
        {
          targetLinearVelocity  = Vector3::ZERO;
          targetAngularVelocity = Vector3::ZERO;
          robot->handleEvent (new VelocityEvent (targetLinearVelocity, targetAngularVelocity));
        }
        break;
      case 'y':
      case 'Y':
        {
          robot->handleEvent (new KickerEvent (true));
          //isKickerEnabled = !isKickerEnabled;
          //robot->handleEvent (new KickerEvent (isKickerEnabled));
        }
        break;
      case 'x':
      case 'X':
        {
          robot->handleEvent (new RotationEvent (Matrix33 (0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0)));
        }
        break;
      case 'c':
      case 'C':
        {
          robot->handleEvent (new RotationEvent (Matrix33::IDENTITY));
        }
        break;
    }
  }

  switch (cmd) {
    case 'p':
    case 'P':
      {
        engine->setIsPaused (!engine->getIsPaused ());
      }
      break;
    case 'o':
    case 'O':
      {
        engine->setIsPaused        (true);
        engine->setIsTakingOneStep (true);
      }
      break;
    case 'k':
    case 'K':
      {
        Timer::getInstance ()->clearEvents ();

        if (robot) {
          collisionGroup->removeCollider (robot->getModel ());
          delete robot;
          robot = 0;
        } else {
          targetLinearVelocity  = Vector3::ZERO;
          targetAngularVelocity = Vector3::ZERO;
          isDribblerEnabled     = false;
          isKickerEnabled       = false;

          robot = new OmniRobot                        (Vector3 (0.0, 0.0, 0.0));
          Engine::getInstance ()->getRoot ()->addChild (robot->getModel ());
          collisionGroup->addCollider                  (robot->getModel ());
          robot->handleEvent                           (new PositionEvent (Vector3(VDVAR (USER_ROBOT_POSITION))));
        }
      }
      break;
    case 'l':
    case 'L':
      {
        Timer::getInstance ()->clearEvents ();

        if (robot) {
          collisionGroup->removeCollider (robot->getModel ());
          delete robot;
          robot = 0;
        } else {
          targetLinearVelocity  = Vector3::ZERO;
          targetAngularVelocity = Vector3::ZERO;
          isDribblerEnabled     = false;
          isKickerEnabled       = false;

          robot = new DiffRobot                        (Vector3 (0.0, 0.0, 0.0));
          Engine::getInstance ()->getRoot ()->addChild (robot->getModel ());
          collisionGroup->addCollider                  (robot->getModel ());
          robot->handleEvent                           (new PositionEvent (Vector3(VDVAR (USER_ROBOT_POSITION))));
        }
      }
      break;
    case ';':
    case ':':
      {
        Timer::getInstance ()->clearEvents ();

        if (ball)
          {
            collisionGroup->removeCollider (ball->getModel ());
            delete ball;
            ball = 0;
          }


        if (engine)
          {
            configreader.UpdateFiles ();

            ball = new Ball ();

            collisionGroup->addCollider  (ball->getModel ());
            engine->getRoot ()->addChild (ball->getModel ());
            ball->handleEvent (new PositionEvent (Vector3(VDVAR (BALL_POSITION))));
          }
      }
      break;
    case 'm':
    case 'M':
      {
        isShowingFps = !isShowingFps;
      }
      break;
    case '0':
      {
        Timer::getInstance ()->clearEvents ();

        if (robot)
          {
            collisionGroup->removeCollider (robot->getModel ());
            delete robot;
            robot = 0;
          }

        for (int i = 0; i < NUM_TEAMS; i++)   {
          for (int j = 0; j < MAX_TEAM_ROBOTS; j++)	{
            if (robotTeams[i][j])
              {
                collisionGroup->removeCollider (robotTeams[i][j]->getModel ());
                delete robotTeams[i][j];
                robotTeams[i][j] = 0;
              }
          }
        }

        if (ball)
          {
            collisionGroup->removeCollider (ball->getModel ());
            delete ball;
            ball = 0;
          }


        if (engine)
          {
            configreader.UpdateFiles ();

            ball = new Ball ();

            collisionGroup->addCollider  (ball->getModel ());
            engine->getRoot ()->addChild (ball->getModel ());
            ball->handleEvent (new PositionEvent (Vector3(VDVAR (BALL_POSITION))));
          }
      }
      break;			
  }
}

void stop ()
{
  delete ball;
  delete robot;

  for (int i = 0; i < NUM_TEAMS; i++) {
    for (int j = 0; j < MAX_TEAM_ROBOTS; j++) {
      delete robotTeams[i][j];
    }
  }

  engine->shutdown ();
}

/*******************************/

int main (int argc, char** argv)
{
  CR_SETUP (Main, TARGET_FPS, CR_DOUBLE);
  CR_SETUP (Main, SKIP_FRAME, CR_INT);
  CR_SETUP (Main, USER_ROBOT_VELOCITY_STEP, CR_DOUBLE);
  CR_SETUP (Main, USER_ROBOT_ANGULAR_VELOCITY_STEP, CR_DOUBLE);
  CR_SETUP (Main, USER_ROBOT_POSITION, CR_DOUBLE);
  CR_SETUP (Main, BALL_POSITION, CR_DOUBLE);
  CR_SETUP (Main, POSITION_SCALE, CR_DOUBLE);
  CR_SETUP (Main, VELOCITY_SCALE, CR_DOUBLE);
  CR_SETUP (Main, ANGULAR_VELOCITY_SCALE, CR_DOUBLE);
  CR_SETUP (Main, X_LOWER_LIMIT, CR_DOUBLE);
  CR_SETUP (Main, X_UPPER_LIMIT, CR_DOUBLE);
  CR_SETUP (Main, Y_LOWER_LIMIT, CR_DOUBLE);
  CR_SETUP (Main, Y_UPPER_LIMIT, CR_DOUBLE);
  CR_SETUP (Main, ROBOT_START_HEIGHT, CR_DOUBLE);
  CR_SETUP (Main, RADIO_LATENCY, CR_DOUBLE);

  char* odehome_env = getenv("ODEHOME");
  std::string odehome_string;

  if (visionServer.connect_server (NET_VISION_PORT) != Socket::Server) {
    fprintf (stderr, "Cannot create vision server on %d\n", NET_VISION_PORT);
    exit    (-1);
  }

  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      robotTeams[t][i]                         = 0;
      visionFrame.config.id[t][i] = -1;
    }
  }

#ifdef USE_OPENGL
  dsFunctions fn;
  fn.version          = DS_VERSION;
  fn.start            = 0;
  fn.step             = &step;
  fn.command          = &command;
  fn.stop             = &stop;

  if (odehome_env == NULL) 
    {
      odehome_string = "../../ode/drawstuff/textures";
    } 
  else 
    {
      odehome_string = odehome_env;
      odehome_string += "/drawstuff/textures";
    }

  fn.path_to_textures = (char*)odehome_string.c_str ();
#endif

  engine = Engine::getInstance ();
  engine->initialize           ();
  engine->setTargetFps         (DVAR (TARGET_FPS));

  collisionGroup = new CollisionGroup ();	
  CollisionManager::getInstance ()->addCollisionGroup (collisionGroup);	

  Node* root     = engine->getRoot  ();
  collisionGroup->addCollidee (root);

  Field* field = new Field ();
  root->addChild (field);

  ball = new Ball             ();
  root->addChild              (ball->getModel ());
  collisionGroup->addCollider (ball->getModel ());	
  ball->handleEvent           (new PositionEvent (Vector3(VDVAR (BALL_POSITION))));

#ifdef USE_OPENGL
  dsSimulationLoop (argc, argv, 640, 480, &fn);
#else

  bool quit = false;
  bool pause = false;

  printf("Starting main loop...\n");
  engine->setIsPaused (false);

  double secs = 0, secs_last = 0;

  while (!quit) {
    for (int i = 0; i < IVAR(SKIP_FRAME); i++) 
      step(pause);
    
    // wait for the time to expire
#if 1

    do {
      timeval curr;
      gettimeofday(&curr, NULL);

      secs = (curr.tv_sec + curr.tv_usec / 1000000.0);

      double dt = (1/30.0 - (secs - secs_last));
      dt = dt * 1e6 - 20e3;
      if (dt > 0.0)
        usleep((int) dt);
    } while (secs < secs_last + 1/30.0);
    secs_last = secs;

#else
    usleep(0);
#endif
  }
  printf("Stopping!!\n");
  stop();
#endif

  return 0;
}

/*******************************/

void makeObservations (Observation& obs)
{
  //printf ("making observations\n");

  Vector3 position;
  Vector3 velocity;
  Vector3 rotation;
  Vector3 angularv;

  // HACK
  double timestamp = (engine->getFrameCount () + 2) / engine->getTargetFps ();
  //double timestamp = engine->getStartTime () + engine->getFrameCount ()  / engine->getTargetFps ();

  obs.timestamp = timestamp;

  //printf ("timestamp: %f\n", timestamp);

  obs.ball.timestamp = timestamp;

  position = ball->getMotionController()->getWorldPosition ();

  //  printf ("ball actual position: %f %f %f\n", position.x, position.y, position.z);

  position.x *= DVAR (POSITION_SCALE);
  position.y *= DVAR (POSITION_SCALE);

  obs.ball.pos = vector2f ((float)position.x , (float)position.y);

  obs.ball.angle = 0.0;
  obs.ball.conf  = 1.0;

  //printf ("ball observed: pos: %f %f angle: %f conf: %f\n", obs.ball.pos.x, obs.ball.pos.y, obs.ball.angle, obs.ball.conf);

  for (unsigned int t = 0; t < NUM_TEAMS; t++) {
    for (unsigned int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      Entity* robot = robotTeams[t][i];

      if (robot) {
        obs.robots[t][i].timestamp = timestamp;

        position = robot->getMotionController ()->getWorldPosition ();
        velocity = robot->getMotionController ()->getWorldVelocity ();

        //printf ("robot[%d][%d] actual postiion: %f %f %f\n", t, i, position.x, position.y, position.z);
        //printf ("robot[%d][%d] actual velocity: %f %f %f %f\n", t, i, velocity.length (), velocity.x, velocity.y, velocity.z);

        position *= DVAR (POSITION_SCALE);
        velocity *= DVAR (VELOCITY_SCALE);

        rotation = robot->getMotionController ()->getWorldRotation ().eulerAngles ();
        angularv = robot->getMotionController ()->getWorldAngularVelocity ();

        //printf ("robots[%d][%d]: %f %f %f", t, i, position.x, position.y, rotation.z);
        //printf (" vel: %f %f abs: %f %f\n", velocity.x, velocity.y, velocity.length (), angularv.z);

        //printf ("robot[%d][%d] actual rotation: %f %f %f\n", t, i, rotation.x, rotation.y, rotation.z);
        //printf ("robot[%d][%d] actual angularv: %f %f %f\n", t, i, angularv.x, angularv.y, angularv.z);

        //rotation.z *= 1000.0;

        //printf ("robot[%d][%d] scaled rotation: %f %f %f\n", t, i, rotation.x, rotation.y, rotation.z);
        //printf ("robot[%d][%d] scaled angularv: %f %f %f\n", t, i, angularv.x, angularv.y, angularv.z);

        obs.robots[t][i].pos   = vector2f ((float)position.x, (float)position.y);
        obs.robots[t][i].angle = (float)rotation.z;
        obs.robots[t][i].conf  = 1.0;

        //printf ("robot[%d][%d]: observed pos: %f %f angle: %f conf: %f\n", t, i, obs.robots[t][i].pos.x, obs.robots[t][i].pos.y, obs.robots[t][i].angle, obs.robots[t][i].conf);
      } else {
        obs.robots[t][i].conf = 0.0;
      }
    }
  }

  obs.ball_kicked = (ball->getMotionController()->getWorldVelocity ().length() > 1.0);

}

/*******************************/

void updateTracking (Observation& obs)
{
  tracker.ball.observe (obs.ball, obs.timestamp);
  
  for (int t = 0; t < NUM_TEAMS; t++)  {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++)  {
      if (0 <= visionFrame.config.id[t][i]) {
        tracker.robots[t][i].observe (obs.robots[t][i], obs.timestamp);
      }
    }
  }
}

/*******************************/

void visionRecieveMessages ()
{


  // HACK: Temporary fix to prevent robots from recieving 2+ velocity commands in one frame
  

  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      robotMsgRecieved[t][i] = false;
    }
  }

  while (visionServer.ready_for_recv ()) {
    VisionMsg msg;
    int        msgtype;
    
    visionServer.recv(&msg, sizeof(msg));
    msgtype = msg.msg.msg_type;
    
    switch (msgtype) {
      case RobotCommandMsgId:
        {	
          //printf ("recieved net_radio_commands\n");
          
          RobotCommandMsg* rcmds = &msg.robot_command;
          int team = rcmds->team;
          
          for (unsigned int i = 0; i < rcmds->num_cmds; i++) {
            int rid  = tracker.id2index[team][rcmds->cmd[i].id];
            
            //printf ("for team: %d robot: %d\n", team, rid);
            
            if ((team < 0) || (team >= NUM_TEAMS)) {
              fprintf (stderr, "bad team: %d\n", team);
              exit    (-1);
            }

            if ((rid < 0) || (rid >= MAX_TEAM_ROBOTS))  {
              fprintf (stderr, "bad id: %d\n", rid);
              exit    (-1);
            }

            Entity* robot = robotTeams[team][rid];

            //            if (robot && !robotMsgRecieved[team][rid]) {	
            if (robot) {	
              robotMsgRecieved[team][rid] = true;

              Vector3 lv (rcmds->cmd[i].vx, rcmds->cmd[i].vy, 0.0);	
              Vector3 av (0.0, 0.0, rcmds->cmd[i].va);	
              //printf ("velocity recieved:\t%f\t%f\t%f\n", lv.x, lv.y, lv.z);

              lv /= DVAR (VELOCITY_SCALE);
              av /= DVAR (ANGULAR_VELOCITY_SCALE);

              Event* event = new VelocityEvent (lv, av);

              Timer::getInstance ()->addEvent (robot, event, DVAR (RADIO_LATENCY));
              //Timer::getInstance ()->addEvent (robot, new VelocityEvent (lv, av), DVAR (RADIO_LATENCY));
              //robot->handleEvent (new VelocityEvent (lv, av));

              Timer::getInstance()->addEvent(robot, new DribblerEvent (rcmds->cmd[i].dribble), 
                                             DVAR (RADIO_LATENCY));

              //robot->handleEvent (new DribblerEvent (rcmds->cmd[i].drib));

              if (rcmds->cmd[i].attr & RobotCommand::AttrKick) {
                Timer::getInstance ()->addEvent (robot, new KickerEvent (true), DVAR (RADIO_LATENCY));
                //robot->handleEvent (new KickerEvent (rcmds->cmd[i].kick));
              }

              tracker.robots[team][rid].command((engine->getFrameCount ()) / engine->getTargetFps (), 
                                                vector3d(rcmds->cmd[i].vx, rcmds->cmd[i].vy, rcmds->cmd[i].va));
            }
          }
        }
        break;

      case RadioControlMsgId:        {
        //printf ("recieved net_radio_control\n");

        RadioControlMsg* rctrl = &msg.radio_control;

        switch (rctrl->control) {
          case VCR_PAUSE:
            engine->setIsPaused (true);
            break;
          case VCR_PLAY:
            engine->setIsPaused (false);
            break;
          case VCR_STEP:
            engine->setIsPaused        (true);
            engine->setIsTakingOneStep (true, IVAR(SKIP_FRAME));
            break;
          case VCR_SLOW:
          case VCR_FFWD:
          case VCR_RWND:
            break;
        }
      } break;


      case VisionConfigMsgId:        {
        Timer::getInstance ()->clearEvents ();

        //printf ("recieved net_vision_config\n");

        VisionConfig * visionConfig = &msg.vision_config.config;

        ball->handleEvent (new PositionEvent (Vector3 (VDVAR (BALL_POSITION))));
        ball->handleEvent (new RotationEvent (Matrix33::IDENTITY));
        ball->handleEvent (new VelocityEvent (Vector3:: ZERO, Vector3:: ZERO));

        tracker.ball.reset ();

        Vector3 color;

        for (int t = 0; t < NUM_TEAMS; t++)	  {
          if (t == 0)   {
            color = Vector3 (0.0, 0.0, 1.0);
          }  else {
            color = Vector3 (1.0, 1.0, 0.0);
          }

          for (int i = 0; i < MAX_TEAM_ROBOTS; i++)   {
            tracker.robots[t][i].reset ();

            if (robotTeams[t][i]){
              collisionGroup->removeCollider (robotTeams[t][i]->getModel ());
              delete robotTeams[t][i];
              robotTeams[t][i] = 0;
            }

            if (0 <= visionConfig->getRobotID(t, i)) {
              Entity* robot;
              switch (visionConfig->getRobotType(t, i)) {
                case ROBOT_TYPE_NONE:
                  fprintf (stderr, "Adding unknown robot type, assuming diff drive.\n");

                case ROBOT_TYPE_DIFF:
                  {
                    //printf ("configuring diff: team: %d robot: %d\n", t, i);

                    robot = robotTeams[t][i] = new DiffRobot (color);
                    engine->getRoot ()->addChild (robot->getModel ());
                  }
                  break;

                case ROBOT_TYPE_OMNI:
                  {
                    robot = robotTeams[t][i] = new OmniRobot (color);
                    engine->getRoot ()->addChild (robot->getModel ());
                  }
                  break;

                default:
                  {
                    fprintf (stderr, "trying to add unknown robot type\n");
                    exit    (-1);
                  }
              }

              Vector3 position (
                                Math::random (DVAR (X_LOWER_LIMIT), DVAR (X_UPPER_LIMIT)), 
                                Math::random (DVAR (Y_LOWER_LIMIT), DVAR (Y_UPPER_LIMIT)), 
                                DVAR (ROBOT_START_HEIGHT));
              robot->handleEvent (new PositionEvent (position));

              //printf ("positioning: %f %f %f\n", position.x, position.y, position.z);

              Matrix33 rotation = Matrix33::rotateAboutZ (Math::random (-Math::TWO_PI, Math::TWO_PI));
              robot->handleEvent (new RotationEvent (rotation));

              //printf ("rotation: %f %f %f\n", euler.x, euler.y, euler.z);

              collisionGroup->addCollider (robot->getModel ());
            }
          }
        }

        memcpy (&visionFrame.config, visionConfig, sizeof (VisionConfigMsg));

        tracker.SetConfig (visionFrame.config, true, true);
      } break;

      case SimRefMsgId:        {
        //printf ("recieved net_vision_ref\n");
        
        visionFrame.ref_state = msg.sim_ref.ref_state;
        visionFrame.kicked = false;
      }  break;
        
      case SimMoveBallMsgId:   {
        //printf ("recieved vsim_moveball\n");

        Vector3 position (msg.sim_move_ball.loc.x, msg.sim_move_ball.loc.y, 0.0);
        //printf ("position command recieved: %f %f\n", position.x, position.y);

        position   /= DVAR (POSITION_SCALE);
        position.z  = ball->getMotionController ()->getWorldPosition ().z;

        ball->handleEvent (new PositionEvent (position));

        //printf ("position command executed: %f %f %f\n", position.x, position.y, position.z);
	
        Vector3 velocity (msg.sim_move_ball.vel.x, msg.sim_move_ball.vel.y, 0.0);

        //printf ("velocity command recieved: %f %f\n", velocity.x, velocity.y);

        velocity /= DVAR (VELOCITY_SCALE);
	
        ball->handleEvent (new VelocityEvent (velocity, Vector3::ZERO));
	
        //printf ("velocity command executed: %f %f %f\n", velocity.x, velocity.y, velocity.z);
	
        tracker.ball.reset ();
      } break;

      case SimMoveRobotMsgId:    {
        //printf ("recieved vsim_moverobot\n");
	
        int team = msg.sim_move_robot.team;
        int rid  = msg.sim_move_robot.id;

        tracker.robots[team][rid].reset ();
	
        Entity* robot = robotTeams[team][rid];
	
        if (robot) {
          Vector3 position (msg.sim_move_robot.loc.x, msg.sim_move_robot.loc.y, 0.0);
	  
          //printf ("position command recieved: %f %f\n", position.x, position.y);
	  
          position   /= DVAR (POSITION_SCALE);
          position.z  = robot->getMotionController ()->getWorldPosition ().z;
	  
          //printf ("position command executed: %f %f %f\n", position.x, position.y, position.z);
	  
          robot->handleEvent (new PositionEvent (position));
	  
          //printf ("angle command recieved: %f\n", angle * Math::RAD_TO_DEG);
	  
          Matrix33 rotation = Matrix33::rotateAboutZ (msg.sim_move_robot.angle);
	  
          robot->handleEvent (new RotationEvent (rotation));
	  
          //printf ("rotation: %f %f %f\n", euler.x, euler.y, euler.z);
        } else {
          fprintf (stderr, "Attempting to move a non-existant robot\n");
        }

      } break;

      default:
        fprintf (stderr, "Unknown vision message type, %d\n", msgtype);
        exit    (-1);
    }
  }
}

/*******************************/

bool visionSendMessages (Observation& obs)
{
  while (visionServer.ready_for_accept ()) 
    visionServer.accept();

  if (!visionServer.ready_for_send()) 
    return false;

  double predictionTime = 0.0;

  visionFrame.msg_type    = VisionFrameMsgId;
  visionFrame.msg_len     = sizeof(visionFrame);
  visionFrame.timestamp   = obs.timestamp;
  visionFrame.ball.vision = obs.ball;

  tracker.GetBallData(visionFrame.ball, predictionTime);

  for (int t = 0; t < NUM_TEAMS; t++) {
    for (int i = 0; i < MAX_TEAM_ROBOTS; i++) {
      visionFrame.robots[t][i].vision = obs.robots[t][i];

      tracker.GetRobotData (visionFrame.robots[t][i], t, i, predictionTime);
    }
  }

  if (obs.ball_kicked) visionFrame.kicked = true;


  visionServer.send(&visionFrame, sizeof (visionFrame));	

  return true;
}

/*******************************/
